#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from __future__ import print_function
import json
from time import sleep
import rospy
import socket
import threading
import os, sys,math
sys.path.append(os.getcwd())
#导入ros消息类型库
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseActionGoal
from math import pi
from math import sin
from math import cos


class SocketServer:
    def __init__(self,IP,PORT):
        # 设置tcp服务端的socket
        self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # 设置重复使用
        self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
        # 绑定地址和端口
        self.server.bind((IP,PORT))
        # 设置被动监听
        self.server.listen(8888)

    def run(self):
        print('等待客户端连接')
        while True:

            # 等待客户端连接
            dataSocket, addr = self.server.accept()
            print("客户端地址：",addr)
            ProcessClient(dataSocket).start()



class ProcessClient(threading.Thread):

    def __init__(self, dataSocket):
        super().__init__()
        self.dataSocket = dataSocket
        self.read_json()
        self.open_node()
        # # 分辨率
        # self.resolution = (640, 480)

    # 初始化节点
    def open_node(self):
            print("初始化节点")
            rospy.init_node("socket_server")
            self.string_pub = rospy.Publisher("socket_string",String,queue_size=1)
            self.point_pub = rospy.Publisher("move_base/goal",MoveBaseActionGoal,queue_size=1)
            self.cmd_vel_pub = rospy.Publisher("cmd_vel",Twist,queue_size=1)

    def init_subscriber(self):
            self.string_sub = rospy.Subscriber("socket_string_ros",String,self.string_callback,queue_size=1,buff_size=52428800)
            self.imu_sub = rospy.Subscriber("imu",Imu,self.imu_callback,queue_size=1,buff_size=52428800)
            self.odom_sub = rospy.Subscriber("odom",Odometry,self.odom_callback,queue_size=1,buff_size=52428800)
            self.cmd_vel_sub = rospy.Subscriber("cmd_vel",Twist,self.cmd_vel_callback,queue_size=1,buff_size=52428800)

    # 读取json文件
    def read_json(self):
        # with open("../socket_server/config/wtopic.json",'r') as load_f:
        with open("/home/zhq/Desktop/socket_server/config/wtopic.json", 'r') as load_f:
            self.wtopic = json.load(load_f)
            print("成功读取数据类型文件")
        self.topic_list = []  # 话题名列表
        # 导入话题名列表
        for i in range(len(self.wtopic)):
            self.topic_list.append(self.wtopic[i]["topic_name"])

    def recv_all(self):
        recv_alldata = self.dataSocket.recv(1024)
        # if not recv_alldata:
        #     break
        recv_alldata = json.loads(recv_alldata.decode())
        print("数据类型为：",type(recv_alldata))
        print("recv_alldata : ",recv_alldata)
        if recv_alldata["topic_name"] in self.topic_list:
            topic_name=recv_alldata["topic_name"]

            if topic_name == "socket_string":
                self.pub_string(recv_alldata)
            elif topic_name == "cmd_vel":
                self.pub_cmdvel(recv_alldata)
            elif topic_name == "move_base/goal":
                self.pub_point(recv_alldata)

    # 接受发送的速度数据
    def pub_cmdvel(self, data):
        msg = Twist()
        msg.linear.x = float(data["linear_x"])
        msg.linear.y = float(data["linear_y"])
        msg.linear.z = 0
        msg.angular.z = float(data["angular_z"])
        self.cmd_vel_pub.publish(msg)
        print("发布cmdvel成功！")

    # 接受坐标点数据
    def pub_point(self, msg):
        x, y, th = msg["point_x"], msg["point_y"], msg["point_angle"]
        print("当前位置：(%s , %s)" % (x, y))
        pose = MoveBaseActionGoal()
        pose.header.stamp = rospy.Time.now()
        pose.goal.target_pose.header.frame_id = 'map'
        pose.goal.target_pose.pose.position.x = float(x)
        pose.goal.target_pose.pose.position.y = float(y)
        # q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
        # th=float(th)


        q = self.rpy2quaternion(0.0, 0.0, float(th) / 180.0 * pi)
        pose.goal.target_pose.pose.orientation.x = q[0]
        pose.goal.target_pose.pose.orientation.y = q[1]
        pose.goal.target_pose.pose.orientation.z = q[2]
        pose.goal.target_pose.pose.orientation.w = q[3]
        self.point_pub.publish(pose)
        print("发布point成功！")

    # 接受imu数据
    def pub_string(self, data):
        # #将接受的话题发布
        msg = String()
        msg.data = data["data"]
        self.string_pub.publish(msg)
        print("发布string成功！")

    def string_callback(self, msg):
        dataindex = self.topic_list.index("socket_string")
        data = self.wtopic[dataindex]  # 初始化发送的数据类型
        data["data"] = msg.data  # 将要发送的数据赋值
        str_jsonstr = json.dumps(data)  # 将数据转化为字符串
        self.dataSocket.send(str_jsonstr.encode())  # 发布数据
        print("收到string话题并发布!")

    def imu_callback(self, msg):
        pass

    def odom_callback(self, msg):
        dataindex = self.topic_list.index("odom")
        data = self.wtopic[dataindex]  # 初始化发送的数据类型
        # print ('数据data',data)
        data["linear_x"] = msg.twist.twist.linear.x
        data["linear_y"] = msg.twist.twist.linear.y
        data["angular_z"] = msg.twist.twist.angular.z
        # print ('数据data',data)
        odom_jsonstr = json.dumps(data)  # 将数据转化为字符串
        self.dataSocket.send(odom_jsonstr.encode())  # 发布数据
        print("收到odom话题并发布,vx:", data["linear_x"])
        # print (odom_jsonstr.encode())
        sleep(0.15)

    def cmd_vel_callback(self, msg):
        dataindex = self.topic_list.index("cmd_vel")
        data = self.wtopic[dataindex]  # 初始化发送的数据类型
        data["linear_x"] = msg.linear.x  # 将要发送的数据赋值
        data["linear_y"] = msg.linear.y
        data["angular_z"] = msg.angular.z
        str_jsonstr = json.dumps(data)  # 将数据转化为字符串
        self.dataSocket.send(str_jsonstr.encode())  # 发布数据
        print("收到cmd_vel话题并发布!")
        sleep(0.15)

    # 欧拉角转四元素
    def rpy2quaternion(self, roll, pitch, yaw):
        yaw = math.radians(yaw)
        pitch = math.radians(pitch)
        roll = math.radians(roll)

        cy, sy = math.cos(yaw * 0.5), math.sin(yaw * 0.5)
        cp, sp = math.cos(pitch * 0.5), math.sin(pitch * 0.5)
        cr, sr = math.cos(roll * 0.5), math.sin(roll * 0.5)

        # 笛卡尔坐标系

        x = sr * cp * cy - cr * sp * sy
        y = cr * sp * cy + sr * cp * sy
        z = cr * cp * sy - sr * sp * cy
        w = cr * cp * cy + sr * sp * sy

        return  x, y, z,w

        #
        # x = sin(pitch / 2) * sin(yaw / 2) * cos(roll / 2) + cos(pitch / 2) * cos(yaw / 2) * sin(roll / 2)
        # y = sin(pitch / 2) * cos(yaw / 2) * cos(roll / 2) + cos(pitch / 2) * sin(yaw / 2) * sin(roll / 2)
        # z = cos(pitch / 2) * sin(yaw / 2) * cos(roll / 2) - sin(pitch / 2) * cos(yaw / 2) * sin(roll / 2)
        # w = cos(pitch / 2) * cos(yaw / 2) * cos(roll / 2) - sin(pitch / 2) * sin(yaw / 2) * sin(roll / 2)
        # return x, y, z, w

    #开启连接
    def run(self):
        self.init_subscriber()
        while not rospy.is_shutdown():
            # 接受数据并进行处理
            print("\n开始读取数据...")
            self.recv_all()

            # try:
            #
            # except:
            #     traceback.print_exc()

if __name__ == '__main__':
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    st.close()
    PORT = 8000
    print("self addr :", IP)
    print("self addr :", PORT)
    server = SocketServer(IP,PORT)
    server.run()
